probe comp

component probe_sim "used for testing probe routines";

pin in  float x-in;
pin in  float y-in;
pin in  float z-in;
pin in  float probe-dia;
pin out bit probe;

param rw float probe-off-x=0.005;
param rw float probe-off-y=-0.007;
param rw float pin-x=5;
param rw float pin-y=5;
param rw float pin-d=2;
param rw float pin-z=-1;
param rw float rect-x=-5;
param rw float rect-y=-5;
param rw float rect-w=3;
param rw float rect-h=2;
param rw float rect-z=1;
param rw float rect-offset=0.25;
param rw float rect-slope=0;
param rw float toolset-x=-5;
param rw float toolset-y=5;
param rw float toolset-d=0.5;
param rw float toolset-z=-5;
param rw float ring-x=5;
param rw float ring-y=-5;
param rw float ring-z=0;
param rw float ring-d=3;
param rw float ring-i=2;

author "alex lait";
license "GPL";
function _;

;;
#include "rtapi_math.h"

FUNCTION(_){

	int probetrip = 0;
	int xtrip = 0;
	int ytrip = 0;


	//pin simulation
	float dist_pin = pow(pow(x_in - pin_x + probe_off_x, 2) + pow(y_in - pin_y + probe_off_y, 2), 0.5);
	if (( dist_pin <= (probe_dia / 2 + pin_d / 2)) && (z_in <= pin_z))
	{
		probetrip = 1;
	}

	//rectangle simulation
	float distx = fabs((x_in - rect_x + probe_off_x) + (y_in - rect_y + probe_off_y) * rect_slope);
	float disty = fabs((y_in - rect_y + probe_off_y) - (x_in - rect_x + probe_off_x) * rect_slope);


	if ((distx <= (probe_dia / 2 + rect_w / 2)) && (disty <= (probe_dia / 2 + rect_h / 2)) && (z_in <= rect_z))
	{
		if ((distx >= (rect_w / 2 - rect_offset - probe_dia / 2)) || (disty >= (rect_h / 2 - rect_offset - probe_dia / 2)))
		{
			probetrip = 1;
		}
	}

	//tool setter simulation
	dist_pin = pow(pow(x_in - toolset_x, 2) + pow(y_in - toolset_y, 2), 0.5);
	if ((dist_pin <= (probe_dia / 2 + toolset_d / 2)) && (z_in <= toolset_z))
	{
		probetrip = 1;
	}

	//ring simulation
	dist_pin = pow(pow(x_in - ring_x + probe_off_x, 2) + pow(y_in - ring_y + probe_off_y, 2), 0.5);
	if ((dist_pin <= (probe_dia / 2 + ring_d / 2)) && (dist_pin >= (ring_i / 2 - probe_dia / 2)) && (z_in <= ring_z))
	{
		probetrip = 1;
	}

	probe = probetrip;

}